/**
 * 该例程产生tf数据，并计算、发布turtle2的速度指令
 * 运行流程：
 *         roscore
 *         rosrun turtlesim turtlesim_node
 *         rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
 *         rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
 *         rosrun learning_tf turtle_tf_listener
 *         rosrun turtlesim turtle_teleop_key
 */

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>							//包含tf下面的广播tf变换头文件
#include <turtlesim/Pose.h>										//海龟的位姿

std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg){ 			//传进来的是全局坐标系下海龟的位姿信息
	static tf::TransformBroadcaster br;
	
	tf::Transform transform;						  			// 初始化tf数据 机器人导论里面一个4*4的转换矩阵，保存了平移关系和旋转关系
	// setOrigin()函数的参数类型需要为tf::Vector3类型
	// 假设是要发布一个子坐标系为”turtle1”父坐标系为“world”，那么其中(msg->x,msg->y,0.0)是指“turtle1”的坐标原点在“world”坐标系下的坐标
	transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );	//设置平移参数
	tf::Quaternion q;											//设置旋转，四元数
	q.setRPY(0, 0, msg->theta);									//完成了两个坐标系之间姿态变换的关系
	transform.setRotation(q);									//通过setRotation设置旋转关系到transform里面

	// 广播world与海龟坐标系之间的tf数据   ros::Time::now():添加时间戳，tf默认是有一个时间的概念的，默认保存10s之内
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

// char** argv:通过这里的输入去判断是turtle1还是turtle2, 任意的节点名在ROS环境当中只能有一个
// 假如这个程序执行两遍的话节点名可能会冲突, 所以会想办法让节点的名字换一个，这样程序就可以执行两遍了，这叫重映射
int main(int argc, char** argv){
	ros::init(argc, argv, "my_tf_broadcaster");
	if (argc != 2){												// 输入参数作为海龟的名字
		ROS_ERROR("need turtle name as argument"); 
		return -1;
	}
	turtle_name = argv[1];
	
	ros::NodeHandle node;
	//这个subscribe会订阅海龟仿真器里面不断发布的海龟的位置消息，在turtlesim中默认发布的是turtle1 /pose，所以他要这样定义
	ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

    // 循环等待回调函数，一旦有海龟位置发布的话，会跳转到这个回调函数里面
	ros::spin();

	return 0;
};


